#include "task.h"
#include "STM32Lib\\stm32f10x.h"
#include "UP_Globle.h" 
#include "UP_Exti.h"
#include "UP_ADC.h"	 
#include "senser_state.h"
#include "pid.h"

uint8_t RestartFlag;		//重启标志位
uint16_t Timekeeping;		//计时
uint8_t s=1;
//uint16_t Count;				//计次
//uint16_t NowValue;			//现在的计数值

//static uint16_t LastValue;	 //上一次计数值

/*******************  巡台  *********************/
void PatrArea(PID_t* pRobot)
{	
//	Count++;	//计数自增
//	if(Count>=LOCKCOUNT)				//判断是否超过计次	
//	{
//		Count=CLEARCOUNT;				//清零计数值
//		LastValue=NowValue;				//交换计数值
//		NowValue=Get_GrayValue();		//获取新的计数值
//		
//		//判断是否停留在原地,避免卡住
//		if(((NowValue-LastValue)>-5&&(NowValue-LastValue)<5)&&UP_ADC_GetValue(2)<ATTACK_VALUE&&GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)==COVEROFF)
//		{
//			/*自发挣脱*/
//			car_control(turn_right);
//			UP_delay_ms(1000);
//		}
//	}
#ifdef FRONTPOSITION
	//前面有目标时计数自增
	if(UP_ADC_GetValue(2)>ATTACK_VALUE||GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)==COVERON)
	{
		Timekeeping++;	//计时
	}
	//否则清零
	else
	{
		Timekeeping=CLEARCOUNT;	//清零计时
	}
	//当计数值超过设定值时，置标志位
	if(Timekeeping>=ATTACKCOUNT1)
	{
		attack_sta=SETFLAG;
		if(Timekeeping>=ATTACKCOUNT2)
		{
			Timekeeping=ATTACKCOUNT2;
			attack_sta=S_SETFLAG;
		}
	}
	//没有超过设定值时，则清除标志位
	else
	{
		attack_sta=RESETFLAG;
	}
#endif

#ifdef VARIABLE
	//判断是否前方有目标
	if(pRobot->Actual==RESETFLAG)
	{
		//通过灰度和标志位来控制速度
		if((Get_GrayValue()>EDGR_VALUE||attack_sta==S_SETFLAG)&&UP_sta==RESETFLAG)
		{
			car_control(high_advance);
		}
		else
		{
			if(attack_sta==SETFLAG&&UP_sta==RESETFLAG)
			{
				car_control(middle_advance);
			}
			else
			{	
				car_control(low_advance);
			}
		}
	}
#elif defined UNIFORM
	//判断标志位
	if(pRobot->Actual==RESETFLAG)
	{
		if(attack_sta==S_SETFLAG&&UP_sta==RESETFLAG)
		{
			car_control(high_advance);
		}
		else if(attack_sta==SETFLAG&&UP_sta==RESETFLAG)
		{
			car_control(middle_advance);
		}
		else
		{
			car_control(low_advance);
		}
	}
#endif
}

/*******************  巡敌  *********************/
void EmyCheck(PID_t* pRobot)
{
	uint8_t i=0;
	//通过查看传感器的接口位置，再进行的设置
	for(i=1;i<15;i++)
	{
		if(i==6)
		{
			i+=2;	//加快遍历速度 
		}
		if(UP_ADC_GetValue(2)>EGEMDISTANCE||GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)==COVERON)	//中间测距检测到有人直接退出循环
		{
			i=2;
			break;
		}
		else if(Get_GrayValue()>EDGR_VALUE&&UP_ADC_GetValue(i)>CTREMDISTANCE)	//判断中间测距是否检测到目标
		{
			break;
		}
		else if(UP_ADC_GetValue(i)>EGEMDISTANCE)	//判断边沿测距是否检测到目标
		{
			break;
		}
	}
#ifdef ALLPOSITION
	//周围有目标时计数自增
	if(i!=15||GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)==COVERON)
	{
		Timekeeping++;	//计时
	}
	//否则清零
	else
	{
		Timekeeping=CLEARCOUNT;	//清零计时
	}
	//当计数值超过设定值时，置标志位
	if(Timekeeping>=ATTACKCOUNT1)
	{
		attack_sta=SETFLAG;
		if(Timekeeping>=ATTACKCOUNT2)
		{
			Timekeeping=ATTACKCOUNT2;
			attack_sta=S_SETFLAG;
		}
	}
	//没有超过设定值时，则清除标志位
	else
	{
		attack_sta=RESETFLAG;	
	}
#endif
	//判断是否前面有目标或周围没有目标
	if(i==15||i==2||GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)==COVERON)
	{
		TIM3->CR1 &= (uint16_t)(~((uint16_t)TIM_CR1_CEN));	//关闭定时器
		pRobot->Actual=RESETFLAG;			//设为默认值0
		pRobot->Error0=RESETFLAG;			//设为默认值0
#ifdef POSITION		
		pRobot->LeftSpeed=MINSPEED;			//设为默认值-1000
		pRobot->RightSpeed=MAXSPEED;		//设为默认值1000
#endif
	}
	else
	{
		TIM3->CR1 |= TIM_CR1_CEN;	//定时器使能
		switch (i)
		{
			/*定义实际值*/
			case 3:pRobot->Actual=-1;break;
			case 4:pRobot->Actual=-2;break;
			case 5:pRobot->Actual=-3;break;
			case 8:pRobot->Actual=-4;break;
			case 9:pRobot->Actual=-5;break;
			
			case  1:pRobot->Actual=1;break;
			case 14:pRobot->Actual=2;break;
			case 13:pRobot->Actual=3;break;
			case 12:pRobot->Actual=4;break;
			case 11:pRobot->Actual=5;break;
			case 10:pRobot->Actual=pRobot->Actual>=0?6:-6;break;	
			
			default:
			break;
		};
	}
}

/*******************  落台判断  *********************/
void IsDownTable(PID_t* pRobot)
{
#ifdef GRAY
//灰度判断
	if((Get_GrayValue()<RESTART_GRAYVALUE&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)==COVERON&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)==COVERON)
		||(UP_ADC_GetValue(2)>1200&&UP_ADC_GetValue(10)>1200))
#elif defined RANGING
//测距判断	
	if(Get_GrayValue()<RESTART_GRAYVALUE
		&&((UP_ADC_GetValue(2)>RESTART_VALUE&&UP_ADC_GetValue(10)>RESTART_VALUE)
		||(UP_ADC_GetValue(13)>RESTART_VALUE&&UP_ADC_GetValue(5)>RESTART_VALUE)
		||(UP_ADC_GetValue(2)>RESTART_VALUE&&UP_ADC_GetValue(13)>RESTART_VALUE)
		||(UP_ADC_GetValue(2)>RESTART_VALUE&&UP_ADC_GetValue(5)>RESTART_VALUE)))
#endif	
	{
		RestartFlag=SETFLAG;		//置重启标志位
		
		//没出问题的情况下尽量关闭中断
//		UP_Exti_ControlIT(EXTI4_IRQn,DISABLE);				//关闭中断
//		UP_Exti_ControlIT(EXTI9_5_IRQn,DISABLE);			//关闭中断
		
		TIM3->CR1 &= (uint16_t)(~((uint16_t)TIM_CR1_CEN));	//关闭定时器
		pRobot->Actual=RESETFLAG;			//设为默认值0
		pRobot->Error0=RESETFLAG;			//设为默认值0
#ifdef POSITION
		pRobot->LeftSpeed=MINSPEED;			//设为默认值-1000
		pRobot->RightSpeed=MAXSPEED;		//设为默认值1000
#endif
//		Count=CLEARCOUNT;					//清零计次
//		NowValue=CLEARCOUNT;				//清零当前灰度值
	}
}

/*******************  重新上台  *********************/
void ComBacToPower()
{
	//判断标志位
	if(RestartFlag==SETFLAG)
	{	
		//台上误判台下的处理
		if(s==SETFLAG)
		{
			car_control(high_reteat);
			UP_delay_ms(350);
			s=RESETFLAG;
		}
		//判断台下
		if(Get_GrayValue()<RESTART_GRAYVALUE||(UP_ADC_GetValue(2)>1200&&UP_ADC_GetValue(10)>1200))
		{
			//判断是否正对台边沿
			if(UP_ADC_GetValue(1)>RESTART_VALUE||UP_ADC_GetValue(3)>RESTART_VALUE)
			{
				car_control(UP_advance);
				UP_delay_ms(1200);
				//通过测距判断是否在角落
				if(UP_ADC_GetValue(2)<CORNER_VALUE)
				{
					//判断是否正对台边沿
					if(UP_ADC_GetValue(10)>JUDGE_VALUE)
					{
						car_control(high_reteat);
						UP_delay_ms(1200);
						car_control(brake);		//避免翘头
						UP_delay_ms(120);
					}
					//判断是否靠近右边
					else if(UP_ADC_GetValue(13)>RESTART_VALUE)
					{
						car_control(high_reteat);
						UP_delay_ms(800);
						car_control(turn_left);
						UP_delay_ms(240);
					}
					//判断是否靠近边沿右边
					else
					{
						car_control(high_reteat);
						UP_delay_ms(800);
						car_control(turn_right);
						UP_delay_ms(240);
					}
				}
			}
			else
			{
				/*寻找擂台边沿*/
				car_control(Round_right);
				while((UP_ADC_GetValue(1)<RESTART_VALUE||UP_ADC_GetValue(3)<RESTART_VALUE)&&Get_GrayValue()<RESTART_GRAYVALUE);
			}
		}
		else
		{
			RestartFlag=RESETFLAG;
			TIM3->CR1 |= TIM_CR1_CEN;					//使能定时器
		}
		//判断是否上台
		if(Get_GrayValue()>RESTART_GRAYVALUE&&UP_ADC_GetValue(2)<RESTART_VALUE&&GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)==COVEROFF)
		{
			RestartFlag=RESETFLAG;
			
			EXTI->PR=EXTI_Line4;						//清除标志位
			EXTI->PR=EXTI_Line5;						//清除标志位
			
//			UP_Exti_ControlIT(EXTI4_IRQn,ENABLE);		//使能中断
//			UP_Exti_ControlIT(EXTI9_5_IRQn,ENABLE);		//使能中断
			
			TIM3->CR1 |= TIM_CR1_CEN;					//使能定时器
		}
	}
}

/*******************  登台  *********************/
void OnStage_Right()
{
	car_control(high_reteat);
	UP_delay_ms(1600);
	car_control(brake);		//避免翘头
	UP_delay_ms(120);
	car_control(turn_right);
	UP_delay_ms(360);
}

void OnStage_Left()
{
	car_control(high_reteat);
	UP_delay_ms(1600);
	car_control(brake);		//避免翘头
	UP_delay_ms(120);
	car_control(turn_left);
	UP_delay_ms(360);
}
